#include <osgDB/ReadFile>
#include <osgDB/WriteFile>
#include <Trajectory3D.h>
#include <osg/Switch>
#include <Point3D.h>
#include <osgFX/Outline>
#include <SceneViewerFunctions.h>
#include <CallBackTest.h>
Trajectory3D::Trajectory3D() : osg::MatrixTransform()
{
    m_animationPath = new osg::AnimationPath;
    m_animationPath->setLoopMode(osg::AnimationPath::LOOP);
    m_durationTime = 3;
    m_mtPtr = NULL;
    m_animatEnable = true;
    m_timeOffset = 0;
    buildChildNode();
}

Trajectory3D::Trajectory3D(const Trajectory3D & trajectory3D, const osg::CopyOp &copyop) : MatrixTransform(trajectory3D,copyop)
{
}

Trajectory3D::~Trajectory3D()
{

}

/**/
bool Trajectory3D::addChild(osg::Node *child)
{
    bool result = false;
    Point3D* pointPtr = dynamic_cast<Point3D*>(child);
    if(pointPtr != NULL)
    {
        result = osg::MatrixTransform::addChild(child);

    }
    else
    {
        osg::Geode* geodePtr = dynamic_cast<osg::Geode*>(child);
        if(geodePtr != NULL)
        {
            result = osg::MatrixTransform::addChild(child);
        }
        else
        {
            osgFX::Outline* outLinePtr = dynamic_cast<osgFX::Outline*>(child);
            if(outLinePtr != NULL)
            {
                result = osg::MatrixTransform::addChild(child);
            }
        }
    }
    updatePointVector();
    m_geom->setVertexArray(m_pointVector);
    m_geom->setPrimitiveSet(0, new osg::DrawArrays( osg::PrimitiveSet::LINE_STRIP, 0,  m_pointVector->size() ) );
    m_geom->dirtyDisplayList();
    m_geom->dirtyBound();
    return result;
}

bool Trajectory3D::removeChildren(unsigned int pos, unsigned int numChildrenToRemove)
{
    /*
    for(int i = pos; i < numChildrenToRemove + pos; i++)
    {
        Point3D* point3d = dynamic_cast<Point3D*>(this->getChild(i));
        if(point3d != NULL)
        {

        }
    }*/

    /**/
    if(numChildrenToRemove == 1)
    {
        Point3D* point3d = dynamic_cast<Point3D*>(this->getChild(pos));
        if(point3d != NULL)
        {
            bool result = osg::MatrixTransform::removeChildren(pos, numChildrenToRemove);
            return result;
        }
    }

    bool result = osg::MatrixTransform::removeChildren(pos, numChildrenToRemove);
    updatePointVector();
    m_geom->setPrimitiveSet(0, new osg::DrawArrays( osg::PrimitiveSet::LINE_STRIP, 0,  m_pointVector->size() ) );
    m_geom->dirtyDisplayList();
    m_geom->dirtyBound();

    if(m_geode->getNumParents() == 0 || m_geode->getParent(0) != this)
    {
        this->addChild(m_geode);
    }

    return result;
}

void Trajectory3D::updatePointPosition(Point3D *pointPtr)
{
    std::cout<<"Trajectory3D::updatePointPosition"<<"index ="<<pointPtr->getIndexInTrajectory3D()<<std::endl;
    std::cout<<"Trajectory3D::updatePointPosition"<<"pointVector ="<<m_pointVector->size()<<std::endl;
    m_pointVector->at(pointPtr->getIndexInTrajectory3D()) = pointPtr->getPosition();

    m_geom->setPrimitiveSet(0, new osg::DrawArrays( osg::PrimitiveSet::LINE_STRIP, 0,  m_pointVector->size() ) );
    m_geom->dirtyDisplayList();
    m_geom->dirtyBound();

    m_matrixVector[pointPtr->getIndexInTrajectory3D()] = pointPtr->getMatrix();
    updateAnimationPath();
}

void Trajectory3D::setLineWidth(double lineWidth)
{
    m_lineWidth->setWidth(lineWidth);
    m_geom->getOrCreateStateSet()->setAttributeAndModes(m_lineWidth.get (),osg::StateAttribute::ON);

}

double Trajectory3D::getLineWidth()
{
    return m_lineWidth->getWidth();
}

void Trajectory3D::setLineColor(const osg::Vec4 &lineColor)
{
    m_colorArray->at(0) = lineColor;
    m_geom->dirtyDisplayList();
}

osg::Vec4 Trajectory3D::getLineColor()
{
    return m_colorArray->at(0);
}

void Trajectory3D::setDurationTime(double durationTime)
{
    m_durationTime = durationTime;
    updateAnimationPath();
}

double Trajectory3D::getDurationTime()
{
    return m_durationTime;
}

void Trajectory3D::setMTName(const std::string &mtName)
{
    if(m_mtPtr != NULL)
    {
        m_mtPtr->setUpdateCallback(NULL);
    }
    if(mtName.size() == 0) return;

    osg::ref_ptr<osg::Node> findedNode = SceneViewerFunctions::findNodeByName(mtName);
    osg::ref_ptr<osg::MatrixTransform> findedMT =dynamic_cast<osg::MatrixTransform*>(findedNode.get());
    if(findedMT != NULL)
    {
        m_mtPtr = findedMT;
        updateAnimationPath();
    }
    else
    {
        m_mtPtr = NULL;
    }
}

std::string Trajectory3D::getMTName()
{
    std::string mtName;
    if(m_mtPtr != NULL)
    {
        mtName = m_mtPtr->getName();
    }
    else
    {

    }
    return mtName;
}

void Trajectory3D::setMTPtr(osg::ref_ptr<osg::MatrixTransform> mtPtr)
{
    m_mtPtr = mtPtr;
}

osg::ref_ptr<osg::MatrixTransform> Trajectory3D::getMTPtr()
{
    return m_mtPtr;
}

void Trajectory3D::setLoopMode(osg::AnimationPath::LoopMode loopMode)
{
    m_animationPath->setLoopMode(loopMode);
}

osg::AnimationPath::LoopMode Trajectory3D::getLoopMode()
{
    return m_animationPath->getLoopMode();
}

void Trajectory3D::setAnimatEnable(bool boolValue)
{
    m_animatEnable = boolValue;
    updateAnimationPath();

}

bool Trajectory3D::getAnimatEnable()
{
    return m_animatEnable;
}

void Trajectory3D::setTimeOffset(double timeOffset)
{
    m_timeOffset = timeOffset;
    updateAnimationPath();
}

double Trajectory3D::getTimeOffset()
{
    return m_timeOffset;
}

void Trajectory3D::buildChildNode()
{
    //add coordinate
    m_geom = new osg::Geometry;
    m_pointVector = new osg::Vec3Array;
    m_colorArray = new osg::Vec4Array;
    osg::ref_ptr<osg::Vec3Array> normalArray = new osg::Vec3Array;

    m_geom->setVertexArray( m_pointVector.get() );
    m_geom->setColorArray( m_colorArray.get() );
    m_geom->setColorBinding( osg::Geometry::BIND_OVERALL );
    m_geom->setNormalArray( normalArray.get() );
    m_geom->setNormalBinding( osg::Geometry::BIND_OVERALL );

    normalArray->push_back( osg::Vec3( 0.f,  0.f, 1.f ) );

    //set data
    m_colorArray->push_back(osg::Vec4(1,0,0,1));

    //m_pointVector->push_back(osg::Vec3(0,0,0));
    //m_pointVector->push_back(osg::Vec3(0, 0, 1));

    m_geom->addPrimitiveSet(new osg::DrawArrays( osg::PrimitiveSet::LINE_STRIP, 0,  m_pointVector->size() ) );

    //line-size
    m_lineWidth = new osg::LineWidth;
    m_lineWidth ->setWidth(2);
    m_geom->getOrCreateStateSet()->setAttributeAndModes(m_lineWidth.get (),osg::StateAttribute::ON);

    m_geode = new osg::Geode;
    m_geode->addDrawable(m_geom);
    m_geode->setNodeMask(0xfffffffe);//geode will not show on objectInspector
    this->addChild(m_geode);
    //disable light of coordinate
    osg::StateSet* stateset = m_geode->getOrCreateStateSet();
    stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);//这里GL_LIGHTING是系统opengl的头文件GL.h中定义的

}

bool Trajectory3D::checkPoint3DInChildren(osg::Node *child)
{
    bool result = false;
    for(int i= 0 ; i < this->getNumChildren(); i++)
    {
        if(this->getChild(i) == child)
        {
            result = true;
        }
        else
        {
            //this->
        }
    }
    return result;
}

void Trajectory3D::updatePointVector()
{
    m_pointVector->clear();
    m_matrixVector.clear();
    for(int i= 0 ; i < this->getNumChildren(); i++)
    {
        Point3D* point3d = dynamic_cast<Point3D*>(this->getChild(i));

        if(point3d != NULL)
        {
            m_pointVector->push_back(point3d->getPosition());
            point3d->setIndexInTrajectory3D(m_pointVector->size()-1);
            m_matrixVector.push_back(point3d->getMatrix());
        }
        else
        {
            osg::Group* group = dynamic_cast<osg::Group*>(this->getChild(i));
            if(group != NULL && group->getNumChildren() > 0)
            {
                point3d = dynamic_cast<Point3D*>(group->getChild(0));
                if(point3d != NULL)
                {
                    m_pointVector->push_back(point3d->getPosition());
                    point3d->setIndexInTrajectory3D(m_pointVector->size()-1);
                    m_matrixVector.push_back(point3d->getMatrix());
                }
            }
        }
    }
    std::cout<<"Trajectory3D::updatePointVector pointVec size="<<m_pointVector->size()<<std::endl;
    std::cout<<"Trajectory3D::updatePointVector pointVec numChild="<<this->getNumChildren()<<std::endl;

    if(m_pointVector->size() == 2)
    {
        std::cout<<"point="<<m_pointVector->at(0)[0]<<","<<m_pointVector->at(0)[1]<<","<<m_pointVector->at(0)[2]<<std::endl;
        std::cout<<"point="<<m_pointVector->at(1)[0]<<","<<m_pointVector->at(1)[1]<<","<<m_pointVector->at(1)[2]<<std::endl;
    }
updateAnimationPath();
}

void Trajectory3D::updateAnimationPath()
{
    m_animationPath->clear();
    if(m_matrixVector.size() == 0)
    {
        return;
    }
    osg::Vec3 pos;
    osg::Quat rotation;
    double time_step = m_durationTime/(m_matrixVector.size()-1);
    double current_time = 0.0;
    for (int i = 0; i < m_matrixVector.size(); i++)
    {

        pos = m_matrixVector[i].getTrans();
        rotation = m_matrixVector[i].getRotate();
        m_animationPath->insert(current_time, osg::AnimationPath::ControlPoint(pos, rotation) );
        current_time += time_step;
    }


    if(m_animatEnable == true)
    {
        osg::AnimationPathCallback* animationPathCallBack = new osg::AnimationPathCallback(m_animationPath);
        animationPathCallBack->setTimeOffset(m_timeOffset);
        if(m_mtPtr != NULL)
        {
            m_mtPtr->setUpdateCallback(animationPathCallBack);
            m_mtPtr->setDataVariance(osg::Object::DataVariance::DYNAMIC);
        }

    }
    else
    {
        if(m_mtPtr != NULL)
        {
            m_mtPtr->removeUpdateCallback(m_mtPtr->getUpdateCallback());
            m_mtPtr->setDataVariance(osg::Object::DataVariance::STATIC);
        }
    }
}
